/*****************************************************************//**
 * \file   EndToolInterface.h
 * \brief  末端器械接口，所有末端器械的基类
 * 
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/
#pragma once

#include "Eigen/Dense"

#define NOTOOL			0   
#define TOOLTYPE_CAMERA	1    
#define TOOLTYPE_BoplarHook	2
#define TOOLTYPE_4DOF	3 
#define TOOLTYPE_3DOF	4
#define TOOLTYPE_FORCEP 5
#define PI 3.14159265358979323846



//机械臂实时动态数据结构体
struct sEndToolDyParameter
{
	//机械臂自由度
	int iFreedom;

	//机械臂实时各关节角（弧度）
	double dCoo6FJoint[6];

	//机械臂末端实时位置
	double dPosturePx, dPosturePy, dPosturePz;

	//机械臂末端实时姿态
	double dPostureOx, dPostureOy, dPostureOz;
	
	//机械臂末端实时位置与姿态
	Eigen::Matrix4d m4dEndMatrix;

	//机械臂末端所夹持的手术器械位置
	Eigen::Vector3d v3dToolPosture;
	double dToolPx, dToolPy, dToolPz;
};

//电机数据结构体
struct sEndToolMotorDevice
{
	bool isMotorDeviceConnect[5];
	double dMotorState[5];
	Eigen::Matrix4d matrix4dToolEnd;
};

/*
  机械臂实时数据结构
*/
struct sEndToolRealTimeData {
	sEndToolDyParameter sRobotDy;
	sEndToolMotorDevice sMotorDevice;
};

class EndToolInterface
{
public:
	//判断手术器械是否已经初始化
	bool bEndToolInitDone = false;

public:

	virtual Eigen::Matrix4d geToolEndMatrix(Eigen::Matrix4d robotend) { return robotend; }
	virtual Eigen::Matrix4d getRobotNextEnd(Eigen::Matrix4d robotend_current, Eigen::Matrix4d toolend_next, Eigen::Vector3d RCM) {return robotend_current;}
	virtual int getEndToolType() { return 0; }
};
